/************************************************************
 * @file: nvsns_mmlib.h
 * @author: Young Leon, Hao
 * @version: V0.0
 * @data: 2023/07/28
 * @brief: mmlib header file
 * @note:
 * @Copyright (C) 2023 Novosense All rights reserved.
 ************************************************************/

/************************************************************
 ************************************************************
 * @par Edition History
 * -V0.0  2023.07.28
 *        -Initial version for mmlib
 *
 ************************************************************/

#ifndef __MM_LIB_H__
#define __MM_LIB_H__
// motor math library
#include "stdint.h"

typedef int16_t Frac16_t;
typedef int32_t Frac32_t;


//#define __ssat(ARG1,ARG2) ({Frac32_t __RES, __ARG1 = (ARG1); __asm ("ssat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); __RES; } )

#define STATIC_FORCEINLINE static __forceinline //__inline//
    
#define F31_MAX 0x3FFFFFFF
#define F31_MIN 0xC0000000
#define INT16_MAX 32767
#define INT16_MIN -32768

/*! Constant representing the maximal negative value of a signed 16-bit fixed point fractional number, floating point
 * representation. */
#define S16FRACT_MIN  (-1.0)

/*! Constant representing the maximal positive value of a signed 16-bit fixed point fractional
 *  number,  floating point representation. */
#define S16FRACT_MAX  (0.999969482421875)

/*! Constant representing the maximal negative value of a signed 31-bit fixed point fractional number, floating point
 * representation. */
#define S31FRACT_MIN   (-1.0)
/*! Constant representing the maximal positive value of a signed 31-bit fixed point fractional
 *  number,  floating point representation. */
#define S31FRACT_MAX   (0.9999999990686774245171597455788)

/*! Macro converting a signed fractional [-1,1) number into a 16-bit fixed point number in format Q1.15.*/
#define FRAC16(x) ((Frac16_t)((x < S16FRACT_MAX) ? ((x >= S16FRACT_MIN) ? (x * 32768.0) : INT16_MIN) : INT16_MAX))

/*! Macro converting a signed fractional [-1,1) number into a 32-bit fixed point number in format Q2.30.*/
#define FRAC32(x) ((Frac32_t)((x < S31FRACT_MAX) ? ((x >= S31FRACT_MIN) ? (x * 1073741824.0) : F31_MIN) : F31_MAX))


/************************************************************
 * @brief: Abs function for F16
 * @param <Frac16_t> f16In1
 * @return <None>
 ************************************************************/
STATIC_FORCEINLINE Frac16_t AbsF16(Frac16_t f16In1)
{
    return ((f16In1 > 0) ? f16In1: (-f16In1));
}

/************************************************************
 * @brief: Convert F31 into F16
 * @param <Frac32_t> f32In1
 * @return <None>
 ************************************************************/
STATIC_FORCEINLINE Frac16_t F32ConvertF16(Frac32_t f32In1) {return (Frac16_t)( (Frac32_t)f32In1 >> 15u);}

/************************************************************
 * @brief: Multiply without saturation and return F16
 * @param <Frac16_t> f16In1
 * @param <Frac16_t> f16In2
 * @return <None>
 ************************************************************/
STATIC_FORCEINLINE Frac16_t MulF16(Frac16_t f16In1, Frac16_t f16In2)
{
    return ((((int32_t)(f16In1)) * ((int32_t)(f16In2))) >> (15));
}

/************************************************************
 * @brief: Sub with Saturation in F16
 * @param <Frac16_t> f16In1
 * @param <Frac16_t> f16In2
 * @return <None>
 ************************************************************/
STATIC_FORCEINLINE Frac16_t SatSubF16(Frac16_t f16In1, Frac16_t f16In2)
{
    return __ssat((((f16In1) - (f16In2))), 16);
}

/************************************************************
 * @brief: Add with Saturation in F16
 * @param <Frac16_t> f16In1
 * @param <Frac16_t> f16In2
 * @return <None>
 ************************************************************/
STATIC_FORCEINLINE Frac16_t SatAddF16(Frac16_t f16In1, Frac16_t f16In2)
{
    return __ssat((((int32_t)(f16In1) + (int32_t)(f16In2))), 16);
}

#endif
